
#include "cLog.h"
#include "cPath.h"
#include <math.h>
#include <stdio.h>
#include <stdlib.h>

cPath::cPath()
{
	x = -1;
	y = -1;
	xf = -1;
	yf = -1;
	nxf = -1;
	nyf = -1;
	dir = 0;

	AStar = NULL;
	mScene = NULL;
}

cPath::~cPath()
{
	if (AStar) delete AStar;
	AStar = NULL;
	mScene = NULL;
}

void cPath::Make(cScene *scene, int cx, int cy, int cxdest, int cydest)
{
	int *walkabilityMap = NULL;
	int ncx, ncy;

	//Exists movement?
	if((cx != cxdest || cy != cydest) && nxf == -1 && nyf == -1 && (cx != x || cy != y || cxdest != xf || cydest != yf) )
	{
		scene->GetWalkabilityMap(&walkabilityMap);

		if(AStar)
		{
			AStar->EndPathfinder();
			delete AStar;
			AStar = NULL;
		}
		AStar=new cAStar();
		AStar->InitializePathfinder();
		AStar->LoadMap(walkabilityMap, scene->GetSceneArea() );
		//Exists path?
		if(AStar->FindPath(1, cx, cy, cxdest, cydest) )
		{
			mScene = scene;
			x = cx;
			y = cy;
			xf = cxdest;
			yf = cydest;
			
			//1st Direction
			AStar->NextCell(&ncx, &ncy);
			CalcDir(x, y, ncx, ncy);
		}
		else
		{
			//Delete A*
			if(AStar)
			{
				AStar->EndPathfinder();
				delete AStar;
				AStar = NULL;
			}
			//Reset trajectory settings
			dir = STOP;
		}
	}
}

void cPath::ReMake(cScene *scene, int cxdest, int cydest)
{
	mScene = scene;
	nxf = cxdest;
	nyf = cydest;
}

int cPath::NextStep(int *px, int *py, int *cx, int *cy)
{
	int tmpxf, tmpyf;

	switch(dir)
	{
		case N:	(*py)-=STEP_LENGTH;						break;
		case S:	(*py)+=STEP_LENGTH;						break;
		case E:	(*px)+=STEP_LENGTH;						break;
		case O:	(*px)-=STEP_LENGTH;						break;
		case NE:(*py)-=STEP_LENGTH; (*px)+=STEP_LENGTH; break;
		case NO:(*py)-=STEP_LENGTH; (*px)-=STEP_LENGTH; break;
		case SE:(*py)+=STEP_LENGTH; (*px)+=STEP_LENGTH; break;
		case SO:(*py)+=STEP_LENGTH; (*px)-=STEP_LENGTH; break;
	}

	if(((*px) % 32) == 0 && ((*py) % 32) == 0)
	{
		x = (*px)>>5;
		*cx = x;
		y = (*py)>>5;
		*cy = y;

		//Calculate next cell
		if(nxf == -1 && nyf == -1) return NextCell();
		//Recalculate path
		tmpxf= nxf;
		tmpyf = nyf;
		xf = -1;
		yf = -1;
		nxf = -1;
		nyf = -1;
		Make(mScene, x, y, tmpxf, tmpyf);
		xf = tmpxf;
		yf = tmpyf;
	}
	return CONTINUE;
}

int cPath::NextCell()
{	
	int ncx, ncy;

	if(x==xf && y==yf)
	{
		AStar->EndPathfinder();
		delete AStar;
		AStar = NULL;
		return ARRIVE;
	}
	AStar->NextCell(&ncx, &ncy);
	CalcDir(x, y, ncx, ncy);
	return CONTINUE;
}

void cPath::GetDestinationCell(int *cxdest, int *cydest)
{
	*cxdest = xf;
	*cydest = yf;
}

int cPath::Faced()
{
	return dir;
}

bool cPath::IsDone()
{
	return (dir == STOP);
}

void cPath::Done()
{
	dir = STOP;
}

void cPath::CalcDir(int x1, int y1, int x2, int y2)
{
	int sdx,sdy;	//sign movement

	sdx=x2-x1;
	sdy=y2-y1;

	// - Horitzontal
	if(sdy==0)
	{
		if(sdx>0) dir=E;
		else	  dir=O;
	}
	// - Vertical
	else if(sdx==0)
	{
		if(sdy>0) dir=S;
		else	  dir=N;
	}
	// - m=dx/dy=1
	else if(abs(sdx)==abs(sdy))
	{
		if(sdx>0)
		{
			if(sdy>0)	dir=SE;
			else		dir=NE;
		}
		else
		{
			if(sdy>0)	dir=SO;
			else		dir=NO;
		}
	}
}